function [y_til, S, flag, Sigma] = kf_(SET, Ct, Pt, Dt, H, data, TT)
% Kalman Filter
% 
% Procedure: 
% 1. Initialize the state and forecast error variance
% 2. Predict forecast error and forecast error variance
% 3. Filter state in current period
% 4. Update state with data

n_   = SET.variable.n_ ;
l_   = SET.variable.l_ ;
ss   = SET.ss ;
nobs = SET.nobs ;
flag = 0 ;

M_ = SET.M_ ;
oo_ = SET.oo_ ;

%% Initialize

Omega  = SET.Omega ;

DOmega = Dt(:,:,1)*Omega*Dt(:,:,1)' ;
Sigma = dlyap_doubling(Pt(:,:,1),DOmega);

S     = zeros(nobs,nobs,ss) ;
y_til = zeros(nobs,ss) ; 
x     = zeros(n_-1,nobs) ;
P     = zeros(n_-1,n_-1,nobs) ;
K     = zeros(n_-1,nobs,ss) ;

Ph = Sigma ;
xh = ((eye(n_-1) - Pt(:,:,1))) \ Ct(:,:,1)  ;

P(:,:,1) = Ph ;
x(:,1)   = xh ;

%% Kalman filter

%keyboard

for t=1:ss
    hidx = SET.EST.hidx ;
    if TT.t_f(t)>0 ; hidx = SET.EST.hidx_no_i ; end

    % Update
    y_til(hidx,t) = data(hidx,t) - H(hidx,:)*x(:,t) ;
    S(hidx,hidx,t) = H(hidx,:)*P(:,:,t)*H(hidx,:)' ;
    K(:,hidx,t) = P(:,:,t)*H(hidx,:)' / S(hidx,hidx,t) ;
    xh = x(:,t) + K(:,hidx,t)*y_til(hidx,t);
    Ph = (eye(n_-1) - K(:,hidx,t)*H(hidx,:))*P(:,:,t) ; 
    
	% Predict
    if t<ss 
        x(:,t+1) = Ct(:,:,t+1)+Pt(:,:,t+1)*xh ;
        P(:,:,t+1) = Pt(:,:,t+1)*Ph*Pt(:,:,t+1)' + ...
            Dt(:,:,t+1)*Omega*Dt(:,:,t+1)' ;
    end
end
